#include "motor.h"
#include "delay.h"
#include "RC.h"
#include "Powerdistribution.h"
#include "Mixer.h"
#include "Adp_Adrc.h"

#define Moto_PwmMax 1000

extern PID_TYPE PID_ROL_Rate,PID_PIT_Rate,PID_YAW_Rate;
extern ADP_ADRC_Struct ADRC_ROL_RATE,ADRC_PIT_RATE,ADRC_YAW_RATE;

extern RC_Dat RC_Data;
extern uint16_t THR_Out;
extern float w1,w2,w3,w4;
extern float outputs[4];



uint16_t M_PWM1=0; //四电机PWM
uint16_t M_PWM2=0;
uint16_t M_PWM3=0;
uint16_t M_PWM4=0;

uint16_t PID_PWM1,PID_PWM2,PID_PWM3,PID_PWM4;
float Throttle_Corrected;


/*
*功  能：PWM配置
*参  数：RCC_APB1Periph_TIMx(定时器时钟),GPIO_AF_TIMx(复用定时器),TIMx
*返回值：无         
*/
void Pwm_Config(uint32_t RCC_APB1Periph_TIMx,uint8_t GPIO_AF_TIMx,TIM_TypeDef* TIMx)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef TIM_OCInitStructure;  

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIMx, ENABLE); //定时器时钟使能

	RCC_AHB1PeriphClockCmd(RCC_PWM_GPIO, ENABLE); //PWM端口时钟使能 

	//GPIO配置
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用模式
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //速度100M
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推完输出
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉输入
	//引脚初始化  
	GPIO_InitStructure.GPIO_Pin = PWM_GPIOA_Pin;
	GPIO_Init(GPIOA, &GPIO_InitStructure); 

	GPIO_InitStructure.GPIO_Pin = PWM_GPIOB_Pin ;
	GPIO_Init(GPIOB, &GPIO_InitStructure); 

	//连接 TIMx 的通道到 AF2 
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_TIMx);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_TIMx); 
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource0, GPIO_AF_TIMx);
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource1, GPIO_AF_TIMx); 


	//更新中断时间 Tout = (ARR-1)*(PSC-1)/CK_INT
	//TIM2~TIM5时钟来源于APB1*2
	TIM_TimeBaseStructure.TIM_Period = 10000-1; //自动重装载值(arr)
	TIM_TimeBaseStructure.TIM_Prescaler = 21-1; //预分频值(psc)
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIMx, &TIM_TimeBaseStructure);

	//PWM 模式1 通道1
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  //高电平有效

	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;                       //CTRX，根据占空比修改
	TIM_OC1Init(TIMx, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIMx, TIM_OCPreload_Enable); //使能TIM的CCR1预装载寄存器

	//PWM1 模式配置 通道2
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OC2Init(TIMx, &TIM_OCInitStructure);
	TIM_OC2PreloadConfig(TIMx, TIM_OCPreload_Enable);

	//PWM1 模式配置 通道3
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OC3Init(TIMx, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIMx, TIM_OCPreload_Enable);

	//PWM1 模式配置 通道4
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OC4Init(TIMx, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIMx, TIM_OCPreload_Enable);

	//ARR 寄存器预装载
	TIM_ARRPreloadConfig(TIMx, ENABLE);

	//TIM_PrescalerConfig(TIMx, PrescalerValue, TIM_PSCReloadMode_Immediate);

	TIM_Cmd(TIMx,ENABLE); //使能TIMx 
}

/*****************************************************************************
*函  数：void Moto_Pwm(int16_t MOTO1_PWM,int16_t MOTO2_PWM,int16_t MOTO3_PWM,int16_t MOTO4_PWM)
*功  能：电机要输出数值转换成PWM波形输出
*参  数：MOTO1_PWM 电机1
*        MOTO2_PWM 电机2
*        MOTO3_PWM 电机3
*        MOTO3_PWM 电机4
*返回值：无 
*备  注：无
*****************************************************************************/
void Motor_Pwm(uint16_t MOTO1_PWM,uint16_t MOTO2_PWM,uint16_t MOTO3_PWM,uint16_t MOTO4_PWM,TIM_TypeDef* TIMx)
{		
	if(MOTO1_PWM>Moto_PwmMax)	MOTO1_PWM = Moto_PwmMax;
	if(MOTO2_PWM>Moto_PwmMax)	MOTO2_PWM = Moto_PwmMax;
	if(MOTO3_PWM>Moto_PwmMax)	MOTO3_PWM = Moto_PwmMax;
	if(MOTO4_PWM>Moto_PwmMax)	MOTO4_PWM = Moto_PwmMax;
     
    TIMx->CCR1 = (uint32_t)(4.0f*MOTO1_PWM+4000);  //1号电机，对应于实物标签PWM4
	TIMx->CCR2 = (uint32_t)(4.0f*MOTO2_PWM+4000);
	TIMx->CCR3 = (uint32_t)(4.0f*MOTO3_PWM+4000);
	TIMx->CCR4 = (uint32_t)(4.0f*MOTO4_PWM+4000);   
}

/*****************************************************************************
*函  数：void Motor_off(void)
*功  能：关闭所有电机
*参  数：无
*返回值：无 
*备  注：无
*****************************************************************************/
void Motor_off(void)  
{   
    M_PWM1 = 0;
    M_PWM2 = 0;
    M_PWM3 = 0;
    M_PWM4 = 0;    
    Motor_Pwm(M_PWM1,M_PWM2,M_PWM3,M_PWM4,TIM3);
}

/*****************************************************************************
*函  数：void Motor_Control(void)
*功  能：电机控制
*****************************************************************************/
void Motor_Control(void)
{
	if( (sys_status == Danger)||(RC_Data.LockFlag==1) )   //危险，给爷撤 &
    {
        Motor_off(); 
        return;
    } 
    else if(RC_Data.LockFlag==0)      //双重保险
    {
        if(RC_Data.RC_THROTTLE<100)   //低油门
        {    
            Motor_off();
        }        
        else
        {      
                
//			M_PWM1 = (uint16_t)w1;   
//            M_PWM2 = (uint16_t)w2;   
//            M_PWM3 = (uint16_t)w3;   
//            M_PWM4 = (uint16_t)w4;
			
//			PID_PWM1 = THR_Out + PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;   
//            PID_PWM2 = THR_Out - PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;   
//            PID_PWM3 = THR_Out - PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;   
//            PID_PWM4 = THR_Out + PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;
			
			
			//Throttle_Corrected = Power_Distribution();
			
//			if(RC_Data.Althold_Flag==0)
//			{
//				PX4_Mixer(1);
//				M_PWM1 =  (uint16_t)(outputs[0]*1000.f);
//				M_PWM2 =  (uint16_t)(outputs[1]*1000.f);
//				M_PWM3 =  (uint16_t)(outputs[2]*1000.f);
//				M_PWM4 =  (uint16_t)(outputs[3]*1000.f);
//			}
//			else 
//			{
//				M_PWM1 = THR_Out + PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;   
//				M_PWM2 = THR_Out - PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;   
//				M_PWM3 = THR_Out - PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;   
//				M_PWM4 = THR_Out + PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;
	
			
//			}
			
		    //Motor_Pwm(PID_PWM1,PID_PWM2,PID_PWM3,PID_PWM4,TIM3); 
			//Motor_Pwm(M_PWM1,M_PWM2,M_PWM3,M_PWM4,TIM3); 
			
			PWM_Cal(2);
			//Motor_Pwm(M_PWM1,M_PWM2,M_PWM3,M_PWM4,TIM3); 
			Motor_Pwm(0,0,0,0,TIM3); 
        }   
	}    
}


/**
 * @brief  飞行失控监测
 * @param  
 **/
void SafeProtect(void)
{
  
//      float rc_dt=0.0f;
//      rc_dt = micros()-RC_Data.RC_nowT;  //计算当前时间与上一次接收到遥控器的时间间隔
	 

}

//当不考虑混控优化时
void PWM_Cal(uint8_t mode)
{
	if(mode==1)  //PID
	{
		M_PWM1 = THR_Out + PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;   
		M_PWM2 = THR_Out - PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;   
		M_PWM3 = THR_Out - PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;   
		M_PWM4 = THR_Out + PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;
		
	}
	else if(mode==2)   //ADRC
	{
		M_PWM1 = THR_Out + ADRC_ROL_RATE.u + ADRC_PIT_RATE.u + PID_YAW_Rate.OutPut;   
		M_PWM2 = THR_Out - ADRC_ROL_RATE.u + ADRC_PIT_RATE.u - PID_YAW_Rate.OutPut;   
		M_PWM3 = THR_Out - ADRC_ROL_RATE.u - ADRC_PIT_RATE.u + PID_YAW_Rate.OutPut;   
		M_PWM4 = THR_Out + ADRC_ROL_RATE.u - ADRC_PIT_RATE.u - PID_YAW_Rate.OutPut;
		
	}
//	else if(mode==2)   //ADRC
//	{
//		M_PWM1 = THR_Out + ADRC_ROL_RATE.u0 + ADRC_PIT_RATE.u0 + PID_YAW_Rate.OutPut;   
//		M_PWM2 = THR_Out - ADRC_ROL_RATE.u0 + ADRC_PIT_RATE.u0 - PID_YAW_Rate.OutPut;   
//		M_PWM3 = THR_Out - ADRC_ROL_RATE.u0 - ADRC_PIT_RATE.u0 + PID_YAW_Rate.OutPut;   
//		M_PWM4 = THR_Out + ADRC_ROL_RATE.u0 - ADRC_PIT_RATE.u0 - PID_YAW_Rate.OutPut;
//		
//	}

}




